#include <QDebug>


template<int m>
Simulator<m>::~Simulator()
{
    qDebug() << "Delete sim1";
    if(this->isRunning())
        this->quit();

    qDebug() << "Delete sim2";
    if(ran)
        delete sys;

    qDebug() << "Delete sim3";
}


template<int m>
Simulator<m>::Simulator(QString filename) : QLabel(filename), dataFile(filename),  objectiveGiven(false)
{
    ran = false;

    QFile file(filename);
    file.open(QIODevice::ReadOnly);

    QString line;
    QTextStream stream(&file);

    line = stream.readLine();
    type = line.toInt();
    QStringList param = stream.readLine().split(':', QString::SkipEmptyParts);
    QStringList moteur = stream.readLine().split(':', QString::SkipEmptyParts);
    QStringList sensors = stream.readLine().split(':', QString::SkipEmptyParts);
    params.clear();
    for(int i = 0 ; i < param.size() ; i++){
        params << param[i].toDouble();
    }
    switch(type)
    {
    case 0:
        sys = new OneServo;
        break;
    case 1:
        sys = new OneWheel;
        break;
    case 2:
        sys = new TwoServos;
        break;
    default:
        sys = new TwoWheels;
        break;
    }
    sys->setParameters(params);


    m1 = moteur[0].toDouble();
    m2 = moteur[1].toDouble();
    m3 = moteur[2].toDouble();
    m4 = moteur[3].toDouble();

    sys->addMotor(m1, m2, m3, m4);

    if(type >1 )
    {
        sys->addMotor(m1, m2, m3, m4);
    }


    s1 = sensors[0].toDouble();
    s2 = sensors[1].toDouble();
    s3 = sensors[2].toDouble();

    sys->addSensor(s1, s2, s3);

    if(type >1 )
    {
        sys->addSensor(s1, s2, s3);
    }

    sys->addPositionSensor(s1, s2, s3);

    file.close();

    sys->checkValue(0);
    if(type >1 )
    {
        sys->checkValue(1);
    }

    ran = true;
}

template<int m>
void Simulator<m>::reinitialize()
{
    objectiveGiven = false;

    sys->setMotorState(0,0);
    if(type > 1)
    {
        sys->setMotorState(1,0);
    }
    sys->setPosition(0,0,0);
}

template<int m>
void Simulator<m>::run(){

    LearningData<m> data(dataFile);

    while(true){
        if(objectiveGiven){
            QList<double> currentState = getState(objective);
            QList<double> action = (data.getCell(currentState)->getVals(currentState)).cons;
            //Explore...
            sys->setConsign(action);
        }
        //Wait that the system move and retrieve the position
        QThread::msleep(50);
        update();
    }

}

template <int n>
inline QList<double> Simulator<n>::getState(QList<double> objective){
    QList<double> currentState = sys->state();
    currentState << objective;
    return currentState;
}


template<int m>
void Simulator<m>::paintEvent(QPaintEvent *)
{
    QPainter painter(this);

    int h = height();
    int w = width();

    painter.translate(w/2,h/2);

    int size;

    if(w > h)
    {
        size = h;
    }
    else
    {
        size = w;
    }

    switch(type)
    {
    case 0:
    {
        double angle = sys->getMotorState(0);
        int armSize = (size*params[2])/50;
        int pieSize = (size*params[2])/120;

        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.setBrush(QColor(0, 180, 80, 100));
        painter.drawPie(-pieSize/2,-pieSize/2,pieSize,pieSize,16*params[0],16*(params[1]-params[0]));

        painter.setBrush(QColor(100, 0, 0, 100));
        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawEllipse(-5,-5,10,10);

        painter.rotate(-angle);

        painter.drawEllipse(armSize-5,-5,10,10);
        painter.setPen(QPen(Qt::black, 3, Qt::SolidLine));
        painter.drawLine(5,0,armSize-5,0);
    }
        break;
    case 1:
    {
        QList<double> pos = sys->getPosition();
        double xMax = ((params[0]+params[1])*size*6)/(220*8);
        double R = (params[1]*size*6)/(220*8*2);
        double x = R+(pos[0]*size*6)/(220*8);

        painter.setBrush(QColor(0, 180, 80, 100));
        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawEllipse(x-R-(3*size)/8,-R,2*R,2*R);

        painter.setPen(QPen(Qt::black, 3, Qt::SolidLine));
        painter.drawLine(-20-(3*size)/8,R,20+(3*size)/8,R);

        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawLine(-(3*size)/8,R,-(3*size)/8,-3*R);
        painter.drawLine(xMax-(3*size)/8,R,xMax-(3*size)/8,-3*R);

        painter.setBrush(QColor(150, 150, 150, 100));
        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.drawRect(-(3*size)/8-20,-3*R,20,4*R);
        painter.drawRect((3*size)/8,-3*R,20+6*size/8 - xMax,4*R);
    }
        break;
    case 2:
    {
        double angle1 = sys->getMotorState(0);
        int armSize1 = (size*params[2])/50;
        int pieSize1 = (size*params[2])/120;
        double angle2 = sys->getMotorState(1);
        int armSize2 = (size*params[5])/50;
        int pieSize2 = (size*params[5])/120;

        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.setBrush(QColor(0, 180, 80, 100));
        painter.drawPie(-pieSize1/2,-pieSize1/2,pieSize1,pieSize1,16*params[0],16*(params[1]-params[0]));

        painter.setBrush(QColor(100, 0, 0, 100));
        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawEllipse(-5,-5,10,10);

        painter.rotate(-angle1);

        painter.drawEllipse(armSize1-5,-5,10,10);
        painter.setPen(QPen(Qt::black, 3, Qt::SolidLine));
        painter.drawLine(5,0,armSize1-5,0);

        painter.translate(armSize1,0);

        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.setBrush(QColor(0, 180, 80, 100));
        painter.drawPie(-pieSize2/2,-pieSize2/2,pieSize2,pieSize2,16*params[3],16*(params[4]-params[3]));

        painter.rotate(-angle2);

        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawEllipse(armSize2-5,-5,10,10);
        painter.setPen(QPen(Qt::black, 3, Qt::SolidLine));
        painter.drawLine(5,0,armSize2-5,0);


    }
        break;
    default:
    {
        QList<double> pos = sys->getPosition();
        double xMax = ((params[0]+params[4])*size*6)/(220*8);
        double yMax = ((params[1]+params[4])*size*6)/(220*8);
        double R = (params[4]*size*6)/(220*8*2);
        double x = R+(pos[0]*size*6)/(220*8);
        double y = R+(pos[1]*size*6)/(220*8);

        painter.setBrush(QColor(0, 180, 80, 100));
        painter.setPen(QPen(Qt::black, 1, Qt::SolidLine));
        painter.drawEllipse(x-R-((3*size)/8),((3*size)/8)-y-R,2*R,2*R);

        painter.setBrush(QColor(0, 0, 0, 0));
        painter.setPen(QPen(Qt::black, 3, Qt::SolidLine));
        painter.drawRect(-(3*size)/8,-(3*size)/8,(6*size)/8,(6*size)/8);
        painter.setPen(QPen(Qt::black, 2, Qt::SolidLine));
        painter.drawRect(-(3*size)/8,((3*size)/8)-yMax,xMax,yMax);

        painter.setBrush(QColor(100, 100, 100, 100));
        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.drawRect(-(3*size)/8-20,-(3*size)/8-20,((6*size)/8)+40,20);
        painter.drawRect(-(3*size)/8-20,(3*size)/8,((6*size)/8)+40,20);
        painter.drawRect(-(3*size)/8-20,-(3*size)/8,20,((6*size)/8));
        painter.drawRect((3*size)/8,-(3*size)/8,20,((6*size)/8));

        painter.setBrush(QColor(150, 150, 150, 100));
        painter.setPen(QPen(QColor(0,0,0,0), 3, Qt::SolidLine));
        painter.drawRect(-(3*size)/8,-((3*size)/8),xMax,((6*size)/8)-yMax);
        painter.drawRect(-(3*size)/8+xMax,-((3*size)/8),((6*size)/8)-xMax,((6*size)/8));
    }
        break;
    }

    painter.end();
}


template<int m>
void Simulator<m>::mousePressEvent( QMouseEvent* ev )
{
    int w = width();
    int h = height();
    int size;

    if(w > h)
    {
        size = h;
    }
    else
    {
        size = w;
    }

    int x = ev->x() - (w/2);
    int y = h/2- ev->y();

    switch(type)
    {
    case 0:
    {
        double r = sqrt(x*x+y*y);
        if(r != 0)
        {
            QList<double> pos;
            double angle = (2*(atan(y/(x+r)))*180)/M_PI;
            if(angle < 0 )
                angle = 360+angle;
            pos << angle/360;
            objective = pos;
            objectiveGiven = true;
        }
    }
        break;
    case 1:
    {
        double xMax = ((params[0]+params[1])*size*6)/(220*8);
        double R = (params[1]*size*6)/(220*8*2);

        int cons = x;
        int consMin = (R-(3*size)/8), consMax = ((xMax - R) - (3*size)/8);

        if( x < consMin )
        {
            cons = consMin;
        }

        if( x > consMax )
        {
            cons = consMax;
        }

        double real = (cons-consMin)/(consMax-consMin);

        QList<double> pos;
        pos.append(real);
        objective = pos;
        objectiveGiven = true;
    }
        break;
    case 2:
    {
        double r = sqrt(x*x+y*y);
        int maxSize = (size*(params[2]+params[5]))/50;

        if(r <= maxSize)
        {
            double lMax = params[2]+params[5];
            QList<double> pos;
            pos.append( ( ((x*50)/size)+lMax )/(2*lMax) );
            pos.append( ( ((y*50)/size)+lMax )/(2*lMax) );
            objective = pos;
            objectiveGiven = true;
        }
        break;
    }
    default:
    {
        double xMax = ((params[0]+params[4])*size*6)/(220*8);
        double yMax = ((params[1]+params[4])*size*6)/(220*8);
        double R = (params[4]*size*6)/(220*8*2);

        int cons = x;
        int consMin = (R-(3*size)/8), consMax = ((xMax - R) - (3*size)/8);

        if( x < consMin )
        {
            cons = consMin;
        }

        if( x > consMax )
        {
            cons = consMax;
        }

        double real = (cons-consMin)/(consMax-consMin);

        QList<double> pos;
        pos.append(real);

        cons = y;
        consMin = -((3*size)/8)+R;
        consMax = yMax - ((3*size)/8)-R;

        if( y < consMin )
        {
            cons = consMin;
        }

        if( y > consMax )
        {
            cons = consMax;
        }

        real = (cons-consMin)/(consMax-consMin);

        pos.append(real);

        objective = pos;
        objectiveGiven = true;
    }
        break;
    }
}
